using BepuUtilities;
using System.Runtime.CompilerServices;
#if MYCODE
using BepuUtilities.Vectors;
#else
using System.Numerics;
#endif

namespace BepuPhysics.Collidables
{
    /// <summary>
    /// 定义能够提供三角形序列的类型。
    /// </summary>
    public interface ITriangleSource
    {
        /// <summary>
        /// 获取序列中的下一个三角形(如果有)。
        /// </summary>
        /// <param name="a">三角形中的第一个顶点。</param>
        /// <param name="b">三角形中的第二个顶点。</param>
        /// <param name="c">三角形中的第三个顶点。</param>
        /// <return>如果存在另一个三角形,则为True,否则为False。</return>
        bool GetNextTriangle(out Vector3 a, out Vector3 b, out Vector3 c);
    }

    /// <summary>
    /// 提供用于计算具有三角形曲面的对象的惯性的辅助对象。
    /// </summary>
    public static class MeshInertiaHelper
    {
        public static void ComputeTetrahedronContribution(in Vector3 a, in Vector3 b, in Vector3 c, float mass, out Symmetric3x3 inertiaTensor)
        {
            // 计算四面体的惯性需要对其体积进行积分。
            // 虽然可以直接给出任意平面方程,但在带有坐标的归一化四面体上进行积分会更方便
            // 位于(0,0,0)、(1,0,0)、(0,1,0)和(0,0,1)。可以使用四面体边将积分位置转换回原始参照系。
            // 也就是说,(1,0,0)在规格化空间中变换为世界空间中的B-A。
            // 为了明确这一点,我们有一个方程式：
            // [1,0,0][B-A]
            // [0,1,0]*变换=[C-A]
            // [0,0,1][D-A]
            // (请注意,您可以将其视为平移等于A的仿射变换。)

            // 由于归一化的边缘方向构成单位矩阵,因此变换就是边缘方向。
            // 因此,给定计算点对惯性张量的贡献的函数f,具有均匀单位密度的归一化四面体的惯性张量为：
            // 积分[f[{i,j,k}],{k,0,1-i-j}],{j,0,1-i}],{i,0,1}];
            // 注意积分边界-它们是归一化四面体的简单形状的结果,使得平面方程更容易处理。
            // 现在,要在真实四面体的形状上积分,需要将规格化坐标转换为世界坐标：
            // 积分[f[{i,j,k}.Transform+A]*Abs[Det[Transform]],{k,0,1-i-j}],{j,0,1-i}],{i,0,1}];

            // 一个关键的区别是包含了变换的雅可比行列式,在本例中,行列式就是四面体的体积乘以6。
            // 对于为什么会存在这样的几何直观,可以考虑归一化积分覆盖一个体积为1/6的四面体。世界空间四面体
            // 体积是Abs[Det[Transform]]/6,所以体积变化的因子正好是Abs[Det[Transform]]。
            // 该术语补偿了集成域中的差异。
            // 它在积分过程中也是恒定的,所以你可以把它拉出来。
            // 类似地,如果你有一个非单位均匀密度,你也会用它乘以积分。

            // 所以,把这些放在一起,并假设伸缩项被提取出来,下面是一大段代码,您可以将其放入Wolfram云中,然后再创建结果：
            // F[{x_,y_,z_}]：={{y^2+z^2,-x*y,-x*z},{-x*y,x^2+z^2,-y*z},{-x*z,-y*z,x^2+y^2}}
            // A={AX,AY,AZ};
            // b={bx,by,bz};
            // C={CX,CY,CZ};
            // D={DX,DY,DZ};
            // AB=b-a;
            // Ac=c-a;
            // AD=d-a;
            // a={ab,ac,ad};
            // 积分[f[{i,j,k}.A+a],{k,0,1-i-j}],{j,0,1-i}],{i,0,1}]

            // 回顾行列式,请注意：
            // 密度*abs(行列式)=密度*体积*6=质量*6
            // 所以实际上不需要计算行列式/体积,因为我们直接得到了质量。
            var diagonalScaling = mass * (6f / 60f);
            inertiaTensor.XX = diagonalScaling * (
                a.Y * a.Y + a.Z * a.Z + b.Y * b.Y + b.Z * b.Z + c.Y * c.Y + c.Z * c.Z +
                b.Y * c.Y + b.Z * c.Z +
                a.Y * (b.Y + c.Y) + a.Z * (b.Z + c.Z));
            inertiaTensor.YY = diagonalScaling * (
                a.X * a.X + a.Z * a.Z + b.X * b.X + b.Z * b.Z + c.X * c.X + c.Z * c.Z +
                b.X * c.X + b.Z * c.Z +
                a.X * (b.X + c.X) + a.Z * (b.Z + c.Z));
            inertiaTensor.ZZ = diagonalScaling * (
                a.X * a.X + a.Y * a.Y + b.X * b.X + b.Y * b.Y + c.X * c.X + c.Y * c.Y +
                b.X * c.X + b.Y * c.Y +
                a.X * (b.X + c.X) + a.Y * (b.Y + c.Y));
            var offScaling = mass * (6f / 120f);
            inertiaTensor.YX = offScaling * (
                -2 * b.X * b.Y - 2 * c.X * c.Y -
                b.Y * c.X - b.X * c.Y -
                a.Y * (b.X + c.X) - a.X * (2 * a.Y + b.Y + c.Y));
            inertiaTensor.ZX = offScaling * (
                -2 * b.X * b.Z - 2 * c.X * c.Z -
                b.Z * c.X - b.X * c.Z -
                a.Z * (b.X + c.X) - a.X * (2 * a.Z + b.Z + c.Z));
            inertiaTensor.ZY = offScaling * (
                -2 * b.Y * b.Z - 2 * c.Y * c.Z -
                b.Z * c.Y - b.Y * c.Z -
                a.Z * (b.Y + c.Y) - a.Y * (2 * a.Z + b.Z + c.Z));
            // TODO：请注意,上面的实现并不完全是最优的。假设现在的表现并不重要。
            // 给出某些凸壳用例,这种情况可能会改变,但在这种情况下,您可能应该一次跳到多个四面体上进行矢量化。
            // (外加一些基本的术语缓存。)

        }

        /// <summary>
        /// 计算第四个顶点位于原点的四面体的有符号体积。
        /// 假设从形状外部可见的三角形在右手坐标中具有顺时针缠绕,或者在左手坐标中具有逆时针缠绕。
        /// </summary>
        /// <param name="a">四面体的第一个顶点。</param>
        /// <param name="b">四面体的第二个顶点。</param>
        /// <param name="c">四面体的第三个顶点。</param>
        /// <return>四面体的体积。</return>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static float ComputeTetrahedronVolume(in Vector3 a, in Vector3 b, in Vector3 c)
        {
            return (1f / 6f) * Vector3.Dot(Vector3.Cross(b, a), c);
        }

        /// <summary>
        /// 假设密度为1,将四面体的惯性贡献与a、b、c和(0,0,0)处的顶点积分。
        /// </summary>
        /// <param name="a">四面体的第一个顶点。</param>
        /// <param name="b">四面体的第二个顶点。</param>
        /// <param name="c">四面体的第三个顶点。</param>
        /// <param name="volume">四面体的体积。</param>
        /// <param name="惯性张量">假设密度为1的此四面体的惯性张量。</param>
        public static void ComputeTetrahedronContribution(in Vector3 a, in Vector3 b, in Vector3 c, out float volume, out Symmetric3x3 inertiaTensor)
        {
            volume = ComputeTetrahedronVolume(a, b, c);
            ComputeTetrahedronContribution(a, b, c, volume, out inertiaTensor);
        }

        /// <summary>
        /// 计算闭合网格的惯性。
        /// 假设从形状外部可见的三角形在右手坐标中具有顺时针缠绕,或者在左手坐标中具有逆时针缠绕。
        /// </summary>
        /// <typeparam name="TTriangleSource">三角源的类型。</typeparam>
        /// <param name="triangleSource">从中检索三角形序列的源。</param>
        /// <param name="quality">要用来缩放惯性张量的网格质量。</param>
        /// <param name="volume">网格的体积。</param>
        /// <param name="惯性">网格的惯性张量。</param>
        public static void ComputeClosedInertia<TTriangleSource>(ref TTriangleSource triangleSource, float mass, out float volume, out Symmetric3x3 inertia) where TTriangleSource : ITriangleSource
        {
            volume = 0;
            Symmetric3x3 summedContributions = default;
            while (triangleSource.GetNextTriangle(out var a, out var b, out var c))
            {
                ComputeTetrahedronContribution(a, b, c, out var tVolume, out var tContribution);
                Symmetric3x3.Add(summedContributions, tContribution, out summedContributions);
                volume += tVolume;
            }
            Symmetric3x3.Scale(summedContributions, mass / volume, out inertia);
        }

        /// <summary>
        /// 计算闭合网格的惯性。
        /// 假设从形状外部可见的三角形在右手坐标中具有顺时针缠绕,或者在左手坐标中具有逆时针缠绕。
        /// </summary>
        /// <typeparam name="TTriangleSource">三角源的类型。</typeparam>
        /// <param name="triangleSource">从中检索三角形序列的源。</param>
        /// <param name="quality">要用来缩放惯性张量的网格质量。</param>
        /// <param name="volume">网格的体积。</param>
        /// <param name="惯性">网格的惯性张量。</param>
        /// <param name="center">网格的质心。</param>
        public static void ComputeClosedInertia<TTriangleSource>(ref TTriangleSource triangleSource, float mass, out float volume, out Symmetric3x3 inertia, out Vector3 center) where TTriangleSource : ITriangleSource
        {
            volume = 0;
            Symmetric3x3 summedContributions = default;
            center = default;
            while (triangleSource.GetNextTriangle(out var a, out var b, out var c))
            {
                ComputeTetrahedronContribution(a, b, c, out var tVolume, out var tContribution);
                Symmetric3x3.Add(summedContributions, tContribution, out summedContributions);
                volume += tVolume;
                center += (a + b + c) * tVolume;
            }
            var inverseVolume = 1f / volume;
            center *= inverseVolume * 0.25f;
            Symmetric3x3.Scale(summedContributions, mass * inverseVolume, out inertia);
        }

        /// <summary>
        /// 计算闭合网格的质心。
        /// </summary>
        /// <typeparam name="TTriangleSource">三角源的类型。</typeparam>
        /// <param name="triangleSource">从中检索三角形序列的源。</param>
        /// <param name="volume">网格的体积。</param>
        /// <param name="center">网格的质心。</param>
        public static void ComputeClosedCenterOfMass<TTriangleSource>(ref TTriangleSource triangleSource, out float volume, out Vector3 center) where TTriangleSource : ITriangleSource
        {
            center = default;
            volume = 0f;
            while (triangleSource.GetNextTriangle(out var a, out var b, out var c))
            {
                var tVolume = ComputeTetrahedronVolume(a, b, c);
                volume += tVolume;
                center += tVolume * (a + b + c);
            }
            center /= volume * 4;
        }

        /// <summary>
        /// 对单个三角形的惯性贡献进行积分。
        /// </summary>
        /// <param name="a">三角形中的第一个顶点。</param>
        /// <param name="b">三角形中的第二个顶点。</param>
        /// <param name="c">三角形中的第三个顶点。</param>
        /// <param name="mass">三角形的质量。</param>
        /// <param name="惯性张量">三角形的惯性张量。</param>
        public static void ComputeTriangleContribution(in Vector3 a, in Vector3 b, in Vector3 c, float mass, out Symmetric3x3 inertiaTensor)
        {
            // 这遵循与四面体惯性张量计算相同的逻辑,但变换不同。
            // 感兴趣的只有两个维度,但如果我们想要将其表示为3x3线性变换：
            // [B-A]
            // [n]
            // [C-A]
            // 式中N=(Ab X Ac)/||ab x ac||。
            // 换句话说,此变换保持平面法线,以便您可以使用(Ab X Ac)*N计算缩放的三角形面积。
            // 实际上,法线实际上不会出现在我们的计算中,因为我们得到的是明确的质量,而不是从整个区域的密度积分。
            // 所以,把这些放在一起,并假设伸缩项被提取出来,下面是一大段代码,您可以将其放入Wolfram云中,然后再创建结果：
            // F[{x_,y_,z_}]：={{y^2+z^2,-x*y,-x*z},{-x*y,x^2+z^2,-y*z},{-x*z,-y*z,x^2+y^2}}
            // a={ax,ay,az};
            // b={bx,by,bz};
            // C={Cx,Cy,Cz};
            // AB=b-a;
            // Ac=c-a;
            // n=十字[ab,ac]/长度[十字[ab,ac]];
            // a={ab,n,ac};
            // RESULT=INTEGRATE[INTEGRATE[f[{i,0,k}.A+a],{k,0,1-i}],{i,0,1}];
            // 回顾行列式,请注意：
            // 密度*abs(行列式)=密度*体积*2=质量*2
            // 所以实际上不需要计算行列式/面积,因为我们直接得到了质量。
            var diagonalScaling = mass * (2f / 12f);
            inertiaTensor.XX = diagonalScaling * (
                a.Y * a.Y + a.Z * a.Z + b.Y * b.Y + b.Z * b.Z + c.Y * c.Y + c.Z * c.Z +
                a.Y * b.Y + a.Z * b.Z + a.Y * c.Y + b.Y * c.Y + a.Z * c.Z + b.Z * c.Z);
            inertiaTensor.YY = diagonalScaling * (
                a.X * a.X + a.Z * a.Z + b.X * b.X + b.Z * b.Z + c.X * c.X + c.Z * c.Z +
                a.X * b.X + a.Z * b.Z + a.X * c.X + b.X * c.X + a.Z * c.Z + b.Z * c.Z);
            inertiaTensor.ZZ = diagonalScaling * (
                a.X * a.X + a.Y * a.Y + b.X * b.X + b.Y * b.Y + c.X * c.X + c.Y * c.Y +
                a.X * b.X + a.Y * b.Y + a.X * c.X + b.X * c.X + a.Y * c.Y + b.Y * c.Y);
            var offScaling = mass * (2f / 24f);
            inertiaTensor.YX = offScaling * (-a.Y * (b.X + c.X) - b.Y * (2 * b.X + c.X) - (b.X + 2 * c.X) * c.Y - a.X * (2 * a.Y + b.Y + c.Y));
            inertiaTensor.ZX = offScaling * (-a.Z * (b.X + c.X) - b.Z * (2 * b.X + c.X) - (b.X + 2 * c.X) * c.Z - a.X * (2 * a.Z + b.Z + c.Z));
            inertiaTensor.ZY = offScaling * (-a.Z * (b.Y + c.Y) - b.Z * (2 * b.Y + c.Y) - (b.Y + 2 * c.Y) * c.Z - a.Y * (2 * a.Z + b.Z + c.Z));
            // TODO：请注意,上面的实现并不完全是最优的。假设现在的表现并不重要。
            // 给出某些凸壳用例,这种情况可能会改变,但在这种情况下,您可能应该一次跳到多个四面体上进行矢量化。
            // (外加一些基本的术语缓存。)
        }

        /// <summary>
        /// 计算三角形的面积。
        /// </summary>
        /// <param name="a">三角形中的第一个顶点。</param>
        /// <param name="b">三角形中的第二个顶点。</param>
        /// <param name="c">三角形中的第三个顶点。</param>
        /// <return>三角形的面积。</return>
        public static float ComputeTriangleArea(in Vector3 a, in Vector3 b, in Vector3 c)
        {
            return 0.5f * Vector3.Cross(b - a, c - a).Length(); // 不是很快,但是同样,我们假设性能与网格惯性辅助对象无关。
        }

        /// <summary>
        /// 假定密度为1,对单个三角形的惯性贡献进行积分。
        /// </summary>
        /// <param name="a">三角形中的第一个顶点。</param>
        /// <param name="b">三角形中的第二个顶点。</param>
        /// <param name="c">三角形中的第三个顶点。</param>
        /// <param name="Area">三角形的面积。</param>
        /// <param name="惯性张量">假设密度为1的三角形的惯性张量。</param>
        public static void ComputeTriangleContribution(in Vector3 a, in Vector3 b, in Vector3 c, out float area, out Symmetric3x3 inertiaTensor)
        {
            area = ComputeTriangleArea(a, b, c);
            ComputeTriangleContribution(a, b, c, area, out inertiaTensor);
        }

        /// <summary>
        /// 计算开放网格的惯性,将其视为三角形汤。
        /// </summary>
        /// <typeparam name="TTriangleSource">三角源的类型。</typeparam>
        /// <param name="triangleSource">从中检索三角形序列的源。</param>
        /// <param name="quality">要用来缩放惯性张量的网格质量。</param>
        /// <param name="惯性">网格的惯性张量。</param>
        public static void ComputeOpenInertia<TTriangleSource>(ref TTriangleSource triangleSource, float mass, out Symmetric3x3 inertia) where TTriangleSource : ITriangleSource
        {
            float area = 0f;
            inertia = default;
            while (triangleSource.GetNextTriangle(out var a, out var b, out var c))
            {
                ComputeTriangleContribution(a, b, c, out var tArea, out var tContribution);
                area += tArea;
                Symmetric3x3.Add(tContribution, inertia, out inertia);
            }
            Symmetric3x3.Scale(inertia, mass / area, out inertia);
        }

        /// <summary>
        /// 计算开放网格的惯性,将其视为三角形汤。
        /// </summary>
        /// <typeparam name="TTriangleSource">三角源的类型。</typeparam>
        /// <param name="triangleSource">从中检索三角形序列的源。</param>
        /// <param name="quality">要用来缩放惯性张量的网格质量。</param>
        /// <param name="惯性">网格的惯性张量。</param>
        /// <param name="center">网格的质心。</param>
        public static void ComputeOpenInertia<TTriangleSource>(ref TTriangleSource triangleSource, float mass, out Symmetric3x3 inertia, out Vector3 center) where TTriangleSource : ITriangleSource
        {
            center = default;
            float area = 0f;
            inertia = default;
            while (triangleSource.GetNextTriangle(out var a, out var b, out var c))
            {
                ComputeTriangleContribution(a, b, c, out var tArea, out var tContribution);
                area += tArea;
                center += tArea * (a + b + c);
                Symmetric3x3.Add(tContribution, inertia, out inertia);
            }
            var inverseArea = 1f / area;
            center *= inverseArea * (1f / 3f);
            Symmetric3x3.Scale(inertia, mass * inverseArea, out inertia);
        }

        /// <summary>
        /// 计算开放网格的质心,将其视为三角形汤。
        /// </summary>
        /// <typeparam name="TTriangleSource">三角源的类型。</typeparam>
        /// <param name="triangleSource">从中检索三角形序列的源。</param>
        /// <return>网格的质心。</return>
        public static Vector3 ComputeOpenCenterOfMass<TTriangleSource>(ref TTriangleSource triangleSource) where TTriangleSource : ITriangleSource
        {
            Vector3 center = default;
            float area = 0f;
            while (triangleSource.GetNextTriangle(out var a, out var b, out var c))
            {
                var tArea = ComputeTriangleArea(a, b, c);
                area += tArea;
                center += tArea * (a + b + c);
            }
            center /= area * 3;
            return center;
        }

        /// <summary>
        /// 基于偏移参考坐标系计算惯性张量的偏移。
        /// </summary>
        /// <param name="quality">与正在移动的惯性张量关联的质量。</param>
        /// <param name="Offset">从当前惯性参照系到新参照系的偏移。</param>
        /// <param name="inetiaOffset">添加惯性张量以将其移入新参考系的修改。</param>
        public static void GetInertiaOffset(float mass, in Vector3 offset, out Symmetric3x3 inertiaOffset)
        {
            // 就是平行轴定理。
            var squared = offset * offset;
            var diagonal = squared.X + squared.Y + squared.Z;
            inertiaOffset.XX = mass * (squared.X - diagonal);
            inertiaOffset.YX = mass * (offset.X * offset.Y);
            inertiaOffset.YY = mass * (squared.Y - diagonal);
            inertiaOffset.ZX = mass * (offset.X * offset.Z);
            inertiaOffset.ZY = mass * (offset.Y * offset.Z);
            inertiaOffset.ZZ = mass * (squared.Z - diagonal);
        }

    }
}
